#ifndef RSIM_CONSTRAINT_H
#define RSIM_CONSTRAINT_H

/* -------------------------------------------------------------------------- *
 * File: Constraint.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Transform.h"
#include "List.h"
#include "LinAlgDecl.h"
#include "ConstraintUtils.h"

#include <iostream>

namespace RSIM{
class SystemData;
class MultibodySystem;
class State;
class RigidBodyNode;

/** Abstract base constraint class */
class Constraint:public ConstraintUtils{       
        protected:
                
                // All Constaraints have different IDs.
                int ConstraintID_;
                
                // Transform between frames P and M of the parent body
                Transform T_PM_;                                
                
                // Id of the constrained body in parent multibodysystem
                int ParentNodeId_;
                
                // Transform between frames B and F of the child body
                Transform T_BF_;                                
                
                // Id of the constrained body in the child multibodysystem
                int ChildNodeId_;
                
                // Id of the comomn parent
                int CommonParent_;                                
                
                List <int> pumap_;
                
                List <int> cumap_;
                
                // True if the constraint is active
                bool isActive_;
                
                // This is the vector which locates F of child with respect to M of parent
                // expressed in newtonian frame.
                Vect3 p_MF_;                                                                                                
                
                void calcCumulativeBetaForParentBranch();
                
                void calcCumulativeBetaForChildBranch();                
                
                // Transform from frames F to M of the parent body
                // Expressed in F frame of each body
                Transform Parent_Bod_FM_;                                
                
                // Transform from frames F to M of the child body
                // Expressed in F frame of each body
                Transform Child_Bod_FM_;
                
                List <int> Parent_Path_;
                
                List <int> Child_Path_;
                
                // This is the cumulative beta vector associated with the parent branch
                // which locates frame M of the constraint with respect to the inboard 
                // handle of each body. Expressed in Newtonian frame
                Vect3 *Parent_cBetak_;
                
                // This is the cumulative beta vector associated with the child branch
                // which locates frame M of the constraint with respect to the inboard 
                // handle of each body. Expressed in Newtonian frame
                Vect3 *Child_cBetak_;
                
                // This stores the errors in this constraint at position level
                Vector Pos_Error_;
                
                // This stores the errors in this constraint at velocity level
                Vector Vel_Error_;
                
                // Parent Multibody System
                MultibodySystem *pMBS_;
                
                // Parent State
                State *pstate_;
                
                // Child Multibody System
                MultibodySystem *cMBS_;
                
                // Child State
                State *cstate_;
        public:
                /** 
                @param T_PM Parent Transform
                @param T_BF Child Tranform
                */
                Constraint(const Transform& T_PM, MultibodySystem *Parent, const int& ParentNodeId,
                           const Transform& T_BF, MultibodySystem *Child, const int& ChildNodeId, const int& Constraint_ID,
                           State *pstate, State *cstate):
                                ConstraintID_(Constraint_ID), T_PM_(T_PM),ParentNodeId_(ParentNodeId),
                                T_BF_(T_BF),ChildNodeId_(ChildNodeId),isActive_(true),
                                Parent_cBetak_(NULL),Child_cBetak_(NULL),cMBS_(Child),pMBS_(Parent),
                                pstate_(pstate),cstate_(cstate){}

                ~Constraint();
                
                void init();
                
                /** This computes the errors in this constraint at position level 
                and stores the values in Pos_Error_ vector */
                virtual void calcPositionLevelError()=0;                                
                
                /** Returns the number of algebraic constraint imposed by this particular constraint */
                virtual int getNAlgebraicConstraints()const=0;
                
                /** True if the origins of of M and F frames are always coincident */
                virtual bool areOriginsOfFramesMAndFCoincident()const=0;
                
                /** True if the derived class results in constraints on the angular velocity
                of connected bodies*/
                virtual bool imposeConstraintOnAngularVelocity()const=0;
                
                /** True if the derived class results in constraints on the linear velocity
                of connected bodies*/
                virtual bool imposeConstraintOnLinearVelocity()const=0;
                
                virtual void calcConstraintJacobian(const int& ctoumap, Matrix *CJac)const=0;
                
                const Transform& getParentTransformFM()const;
                const Transform& getChildTransformFM()const;
                
                
                Transform& wgetParentTransformFM();
                Transform& wgetChildTransformFM();
                
                const Vector& getPositionErrorAsVector()const;
                const Vector& getVeclocityErrorAsVector()const;
                
                const Transform& getConstraintTransformPM()const;
                const Transform& getConstraintTransformBF()const;
                
                const Vect3* getParentBetak()const;
                const Vect3* getChildBetak()const;
                
                void allocateSpaceForBetak();                
                
                Vect3* wgetParentBetak();
                Vect3* wgetChildBetak();                                
                
                int getParentNodeId()const;                
                
                int getChildNodeId()const;
                
                int getCommonParentId()const;
                
                List <int>& wgetPUMap(){return pumap_;}
                const List <int>& getPUMap()const{return pumap_;}
                
                List <int>& wgetCUMap(){return cumap_;}
                const List <int>& getCUMap()const{return cumap_;}
                
                void setCommonParentId(const int& common_parent);
                
                List <int>& wgetParentPathList();
                const List <int>& getParentPathList()const;
                
                List <int>& wgetChildPathList();
                const List <int>& getChildPathList()const;                                
                
                /** This sets the isActive_ variable to true */
                void activate();
                
                /** This sets the isActive variable to false */
                void deactivate();
                
                /** Query the current state of the constraint */
                bool isConstraintActive()const;
                
                /** This is not yet implemented, but is on the wish list */
                void setRigid()const;
                
                void initConstraintBodyFixedTransforms();
                
                void calcCumulativeBeta();
                
                void updQ(const Vector& delq, int& qid);
                
                const MultibodySystem* getParentSystem()const;                
                const MultibodySystem* getChildSystem()const;
                
                MultibodySystem* wgetParentSystem();
                MultibodySystem* wgetChildSystem();
                
                int getParentSysId()const;
                int getChildSysId()const;
                
}; // class Constraint

inline const MultibodySystem* Constraint::getParentSystem()const{return pMBS_;}                
inline const MultibodySystem* Constraint::getChildSystem()const{return cMBS_;}

inline MultibodySystem* Constraint::wgetParentSystem(){return pMBS_;}
inline MultibodySystem* Constraint::wgetChildSystem(){return cMBS_;}

inline const Vect3* Constraint::getParentBetak()const{return Parent_cBetak_;}
inline Vect3* Constraint::wgetParentBetak(){return Parent_cBetak_;}

inline const Vect3* Constraint::getChildBetak()const{return Child_cBetak_;}
inline Vect3* Constraint::wgetChildBetak(){return Child_cBetak_;}

inline bool Constraint::isConstraintActive()const{return isActive_;}

inline void Constraint::activate(){isActive_ = true;}
inline void Constraint::deactivate(){isActive_= false;}

inline const Transform& Constraint::getParentTransformFM()const{return Parent_Bod_FM_;}
inline Transform& Constraint::wgetParentTransformFM(){return Parent_Bod_FM_;}

inline const Transform& Constraint::getChildTransformFM()const{return Child_Bod_FM_;}            
inline Transform& Constraint::wgetChildTransformFM(){return Child_Bod_FM_;}

inline List <int>& Constraint::wgetParentPathList(){return Parent_Path_;}
inline const List <int>& Constraint::getParentPathList()const{return Parent_Path_;}

inline List <int>& Constraint::wgetChildPathList(){return Child_Path_;}
inline const List <int>& Constraint::getChildPathList()const{return Child_Path_;}

inline int Constraint::getParentNodeId()const{return ParentNodeId_;}
inline int Constraint::getChildNodeId()const{return ChildNodeId_;}

inline int Constraint::getCommonParentId()const{return CommonParent_;}
inline void Constraint::setCommonParentId(const int& i){CommonParent_ = i;}

inline const Transform& Constraint::getConstraintTransformPM()const{return T_PM_;}
inline const Transform& Constraint::getConstraintTransformBF()const{return T_BF_;}

inline const Vector& Constraint::getPositionErrorAsVector()const{return Pos_Error_;}
inline const Vector& Constraint::getVeclocityErrorAsVector()const{return Vel_Error_;}

}// namespace RSIM

#endif